/*
 * Copyright 2006 The Android Open Source Project
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */
#include <souistd.h>
#include "matrix/SCamera.h"

namespace SOUI {

    /**
    *  Use to cast a pointer to a different type, and maintaining strict-aliasing
    */
    template <typename Dst> Dst SkTCast(const void *ptr) {
        union {
            const void *src;
            Dst dst;
        } data;
        data.src = ptr;
        return data.dst;
    }
    
    
    struct SkUnit3D {
        float fX, fY, fZ;
        
        void set(float x, float y, float z) {
            fX = x;
            fY = y;
            fZ = z;
        }
        static float Dot(const SkUnit3D &, const SkUnit3D &);
        static void Cross(const SkUnit3D &, const SkUnit3D &, SkUnit3D *cross);
    };
    
    struct SkPoint3D {
        float    fX, fY, fZ;
        
        void set(float x, float y, float z) {
            fX = x;
            fY = y;
            fZ = z;
        }
        float    normalize(SkUnit3D *) const;
    };
    typedef SkPoint3D SkVector3D;
    
    struct SkMatrix3D {
        float    fMat[3][4];
        
        void reset();
        
        void setRow(int row, float a, float b, float c, float d = 0) {
            SASSERT((unsigned)row < 3);
            fMat[row][0] = a;
            fMat[row][1] = b;
            fMat[row][2] = c;
            fMat[row][3] = d;
        }
        
        void setRotateX(float deg);
        void setRotateY(float deg);
        void setRotateZ(float deg);
        void setTranslate(float x, float y, float z);
        
        void preRotateX(float deg);
        void preRotateY(float deg);
        void preRotateZ(float deg);
        void preTranslate(float x, float y, float z);
        
        void setConcat(const SkMatrix3D &a, const SkMatrix3D &b);
        void mapPoint(const SkPoint3D &src, SkPoint3D *dst) const;
        void mapVector(const SkVector3D &src, SkVector3D *dst) const;
        
        void mapPoint(SkPoint3D *v) const {
            this->mapPoint(*v, v);
        }
        
        void mapVector(SkVector3D *v) const {
            this->mapVector(*v, v);
        }
    };
    
    class SkPatch3D {
      public:
        SkPatch3D();
        
        void    reset();
        void    transform(const SkMatrix3D &, SkPatch3D *dst = NULL) const;
        
        // dot a unit vector with the patch's normal
        float dotWith(float dx, float dy, float dz) const;
        float dotWith(const SkVector3D &v) const {
            return this->dotWith(v.fX, v.fY, v.fZ);
        }
        
      public: // make public for SkDraw3D for now
        SkVector3D  fU, fV;
        SkPoint3D   fOrigin;
        
        friend class SkCamera3D;
    };
    
    class SkCamera3D {
      public:
        SkCamera3D();
        
        void reset();
        void update();
        void patchToMatrix(const SkPatch3D &, SMatrix *matrix) const;
        
        SkPoint3D   fLocation;
        SkPoint3D   fAxis;
        SkPoint3D   fZenith;
        SkPoint3D   fObserver;
        
      private:
        mutable SMatrix    fOrientation;
        mutable bool        fNeedToUpdate;
        
        void doUpdate() const;
    };
    
    class S3DView  {
      public:
        S3DView();
        ~S3DView();
        
        void save();
        void restore();
        
        void translate(float x, float y, float z);
        void rotateX(float deg);
        void rotateY(float deg);
        void rotateZ(float deg);
        
        void setCameraLocation(float x, float y, float z);
        float getCameraLocationX();
        float getCameraLocationY();
        float getCameraLocationZ();
        
        void getMatrix(SMatrix *) const;
        
        float dotWithNormal(float dx, float dy, float dz) const;
        
      private:
        struct Rec {
            Rec        *fNext;
            SkMatrix3D  fMatrix;
        };
        Rec        *fRec;
        Rec         fInitialRec;
        SkCamera3D  fCamera;
    };
    
    
    static float SFloatDotDiv(int count, const float a[], int step_a,
                              const float b[], int step_b,
                              float denom) {
        float prod = 0;
        for (int i = 0; i < count; i++) {
            prod += a[0] * b[0];
            a += step_a;
            b += step_b;
        }
        return prod / denom;
    }
    
    static float SFloatDot(int count, const float a[], int step_a,
                           const float b[], int step_b) {
        float prod = 0;
        for (int i = 0; i < count; i++) {
            prod += a[0] * b[0];
            a += step_a;
            b += step_b;
        }
        return prod;
    }
    
///////////////////////////////////////////////////////////////////////////////

    float SkPoint3D::normalize(SkUnit3D *unit) const {
        float mag = SFloatSqrt(fX * fX + fY * fY + fZ * fZ);
        if (mag) {
            float scale = SFloatInvert(mag);
            unit->fX = fX * scale;
            unit->fY = fY * scale;
            unit->fZ = fZ * scale;
        } else {
            unit->fX = unit->fY = unit->fZ = 0;
        }
        return mag;
    }
    
    float SkUnit3D::Dot(const SkUnit3D &a, const SkUnit3D &b) {
        return a.fX * b.fX + a.fY * b.fY + a.fZ * b.fZ;
    }
    
    void SkUnit3D::Cross(const SkUnit3D &a, const SkUnit3D &b, SkUnit3D *cross) {
        SASSERT(cross);
        
        // use x,y,z, in case &a == cross or &b == cross
        
        float x = a.fY * b.fZ - a.fZ * b.fY;
        float y = a.fZ * b.fX - a.fX * b.fY;
        float z = a.fX * b.fY - a.fY * b.fX;
        
        cross->set(x, y, z);
    }
    
///////////////////////////////////////////////////////////////////////////////

    SkPatch3D::SkPatch3D() {
        this->reset();
    }
    
    void SkPatch3D::reset() {
        fOrigin.set(0, 0, 0);
        fU.set(SK_Scalar1, 0, 0);
        fV.set(0, -SK_Scalar1, 0);
    }
    
    void SkPatch3D::transform(const SkMatrix3D &m, SkPatch3D *dst) const {
        if (dst == NULL) {
            dst = (SkPatch3D *)this;
        }
        m.mapVector(fU, &dst->fU);
        m.mapVector(fV, &dst->fV);
        m.mapPoint(fOrigin, &dst->fOrigin);
    }
    
    float SkPatch3D::dotWith(float dx, float dy, float dz) const {
        float cx = SFloatMul(fU.fY, fV.fZ) - SFloatMul(fU.fZ, fV.fY);
        float cy = SFloatMul(fU.fZ, fV.fX) - SFloatMul(fU.fX, fV.fY);
        float cz = SFloatMul(fU.fX, fV.fY) - SFloatMul(fU.fY, fV.fX);
        
        return SFloatMul(cx, dx) + SFloatMul(cy, dy) + SFloatMul(cz, dz);
    }
    
///////////////////////////////////////////////////////////////////////////////

    void SkMatrix3D::reset() {
        memset(fMat, 0, sizeof(fMat));
        fMat[0][0] = fMat[1][1] = fMat[2][2] = SK_Scalar1;
    }
    
    void SkMatrix3D::setTranslate(float x, float y, float z) {
        memset(fMat, 0, sizeof(fMat));
        fMat[0][0] = x;
        fMat[1][1] = y;
        fMat[2][2] = z;
    }
    
    void SkMatrix3D::setRotateX(float degX) {
        float    s, c;
        
        s = SMatrix::SFloatSinCos(SkDegreesToRadians(degX), &c);
        this->setRow(0, SK_Scalar1, 0, 0);
        this->setRow(1, 0, c, -s);
        this->setRow(2, 0, s, c);
    }
    
    void SkMatrix3D::setRotateY(float degY) {
        float    s, c;
        
        s = SMatrix::SFloatSinCos(SkDegreesToRadians(degY), &c);
        this->setRow(0, c, 0, -s);
        this->setRow(1, 0, SK_Scalar1, 0);
        this->setRow(2, s, 0, c);
    }
    
    void SkMatrix3D::setRotateZ(float degZ) {
        float    s, c;
        
        s = SMatrix::SFloatSinCos(SkDegreesToRadians(degZ), &c);
        this->setRow(0, c, -s, 0);
        this->setRow(1, s, c, 0);
        this->setRow(2, 0, 0, SK_Scalar1);
    }
    
    void SkMatrix3D::preTranslate(float x, float y, float z) {
        float col[3] = { x, y, z};
        
        for (int i = 0; i < 3; i++) {
            fMat[i][3] += SFloatDot(3, &fMat[i][0], 1, col, 1);
        }
    }
    
    void SkMatrix3D::preRotateX(float degX) {
        SkMatrix3D m;
        m.setRotateX(degX);
        this->setConcat(*this, m);
    }
    
    void SkMatrix3D::preRotateY(float degY) {
        SkMatrix3D m;
        m.setRotateY(degY);
        this->setConcat(*this, m);
    }
    
    void SkMatrix3D::preRotateZ(float degZ) {
        SkMatrix3D m;
        m.setRotateZ(degZ);
        this->setConcat(*this, m);
    }
    
    void SkMatrix3D::setConcat(const SkMatrix3D &a, const SkMatrix3D &b) {
        SkMatrix3D  tmp;
        SkMatrix3D *c = this;
        
        if (this == &a || this == &b) {
            c = &tmp;
        }
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                c->fMat[i][j] = SFloatDot(3, &a.fMat[i][0], 1, &b.fMat[0][j], 4);
            }
            c->fMat[i][3] = SFloatDot(3, &a.fMat[i][0], 1,
                                      &b.fMat[0][3], 4) + a.fMat[i][3];
        }
        
        if (c == &tmp) {
            *this = tmp;
        }
    }
    
    void SkMatrix3D::mapPoint(const SkPoint3D &src, SkPoint3D *dst) const {
        float x = SFloatDot(3, &fMat[0][0], 1, &src.fX, 1) + fMat[0][3];
        float y = SFloatDot(3, &fMat[1][0], 1, &src.fX, 1) + fMat[1][3];
        float z = SFloatDot(3, &fMat[2][0], 1, &src.fX, 1) + fMat[2][3];
        dst->set(x, y, z);
    }
    
    void SkMatrix3D::mapVector(const SkVector3D &src, SkVector3D *dst) const {
        float x = SFloatDot(3, &fMat[0][0], 1, &src.fX, 1);
        float y = SFloatDot(3, &fMat[1][0], 1, &src.fX, 1);
        float z = SFloatDot(3, &fMat[2][0], 1, &src.fX, 1);
        dst->set(x, y, z);
    }
    
///////////////////////////////////////////////////////////////////////////////

    SkCamera3D::SkCamera3D() {
        this->reset();
    }
    
    void SkCamera3D::reset() {
        fLocation.set(0, 0, -SkIntToScalar(576));   // 8 inches backward
        fAxis.set(0, 0, SK_Scalar1);                // forward
        fZenith.set(0, -SK_Scalar1, 0);             // up
        
        fObserver.set(0, 0, fLocation.fZ);
        
        fNeedToUpdate = true;
    }
    
    void SkCamera3D::update() {
        fNeedToUpdate = true;
    }
    
    void SkCamera3D::doUpdate() const {
        SkUnit3D    axis, zenith, cross;
        
        fAxis.normalize(&axis);
        
        {
            float dot = SkUnit3D::Dot(*SkTCast<const SkUnit3D *>(&fZenith), axis);
            
            zenith.fX = fZenith.fX - dot * axis.fX;
            zenith.fY = fZenith.fY - dot * axis.fY;
            zenith.fZ = fZenith.fZ - dot * axis.fZ;
            
            SkTCast<SkPoint3D *>(&zenith)->normalize(&zenith);
        }
        
        SkUnit3D::Cross(axis, zenith, &cross);
        
        {
            SMatrix *orien = &fOrientation;
            float x = fObserver.fX;
            float y = fObserver.fY;
            float z = fObserver.fZ;
            
            orien->set(SMatrix::kMScaleX, x * axis.fX - z * cross.fX);
            orien->set(SMatrix::kMSkewX,  x * axis.fY - z * cross.fY);
            orien->set(SMatrix::kMTransX, x * axis.fZ - z * cross.fZ);
            orien->set(SMatrix::kMSkewY,  y * axis.fX - z * zenith.fX);
            orien->set(SMatrix::kMScaleY, y * axis.fY - z * zenith.fY);
            orien->set(SMatrix::kMTransY, y * axis.fZ - z * zenith.fZ);
            orien->set(SMatrix::kMPersp0, axis.fX);
            orien->set(SMatrix::kMPersp1, axis.fY);
            orien->set(SMatrix::kMPersp2, axis.fZ);
        }
    }
    
    void SkCamera3D::patchToMatrix(const SkPatch3D &quilt, SMatrix *matrix) const {
        if (fNeedToUpdate) {
            this->doUpdate();
            fNeedToUpdate = false;
        }
        
        const float *mapPtr = fOrientation.GetData();
        const float *patchPtr;
        SkPoint3D       diff;
        float        dot;
        
        diff.fX = quilt.fOrigin.fX - fLocation.fX;
        diff.fY = quilt.fOrigin.fY - fLocation.fY;
        diff.fZ = quilt.fOrigin.fZ - fLocation.fZ;
        
        dot = SkUnit3D::Dot(*SkTCast<const SkUnit3D *>(&diff),
                            *SkTCast<const SkUnit3D *>(fOrientation.GetData() + 6));
                            
        patchPtr = (const float *)&quilt;
        matrix->set(SMatrix::kMScaleX, SFloatDotDiv(3, patchPtr, 1, mapPtr, 1, dot));
        matrix->set(SMatrix::kMSkewY,  SFloatDotDiv(3, patchPtr, 1, mapPtr + 3, 1, dot));
        matrix->set(SMatrix::kMPersp0, SFloatDotDiv(3, patchPtr, 1, mapPtr + 6, 1, dot));
        
        patchPtr += 3;
        matrix->set(SMatrix::kMSkewX,  SFloatDotDiv(3, patchPtr, 1, mapPtr, 1, dot));
        matrix->set(SMatrix::kMScaleY, SFloatDotDiv(3, patchPtr, 1, mapPtr + 3, 1, dot));
        matrix->set(SMatrix::kMPersp1, SFloatDotDiv(3, patchPtr, 1, mapPtr + 6, 1, dot));
        
        patchPtr = (const float *)(const void *)&diff;
        matrix->set(SMatrix::kMTransX, SFloatDotDiv(3, patchPtr, 1, mapPtr, 1, dot));
        matrix->set(SMatrix::kMTransY, SFloatDotDiv(3, patchPtr, 1, mapPtr + 3, 1, dot));
        matrix->set(SMatrix::kMPersp2, SK_Scalar1);
    }
    
///////////////////////////////////////////////////////////////////////////////

    S3DView::S3DView() {
        fInitialRec.fMatrix.reset();
        fRec = &fInitialRec;
    }
    
    S3DView::~S3DView() {
        Rec *rec = fRec;
        while (rec != &fInitialRec) {
            Rec *next = rec->fNext;
            delete rec;
            rec = next;
        }
    }
    
    void S3DView::save() {
        Rec *rec = new Rec;
        rec->fNext = fRec;
        rec->fMatrix = fRec->fMatrix;
        fRec = rec;
    }
    
    void S3DView::restore() {
        SASSERT(fRec != &fInitialRec);
        Rec *next = fRec->fNext;
        delete fRec;
        fRec = next;
    }
    
    void S3DView::setCameraLocation(float x, float y, float z) {
        // the camera location is passed in inches, set in pt
        float lz = z * 72.0f;
        fCamera.fLocation.set(x * 72.0f, y * 72.0f, lz);
        fCamera.fObserver.set(0, 0, lz);
        fCamera.update();
        
    }
    
    float S3DView::getCameraLocationX() {
        return fCamera.fLocation.fX / 72.0f;
    }
    
    float S3DView::getCameraLocationY() {
        return fCamera.fLocation.fY / 72.0f;
    }
    
    float S3DView::getCameraLocationZ() {
        return fCamera.fLocation.fZ / 72.0f;
    }
    
    void S3DView::translate(float x, float y, float z) {
        fRec->fMatrix.preTranslate(x, y, z);
    }
    
    void S3DView::rotateX(float deg) {
        fRec->fMatrix.preRotateX(deg);
    }
    
    void S3DView::rotateY(float deg) {
        fRec->fMatrix.preRotateY(deg);
    }
    
    void S3DView::rotateZ(float deg) {
        fRec->fMatrix.preRotateZ(deg);
    }
    
    float S3DView::dotWithNormal(float x, float y, float z) const {
        SkPatch3D   patch;
        patch.transform(fRec->fMatrix);
        return patch.dotWith(x, y, z);
    }
    
    void S3DView::getMatrix(SMatrix *matrix) const {
        if (matrix != NULL) {
            SkPatch3D   patch;
            patch.transform(fRec->fMatrix);
            fCamera.patchToMatrix(patch, matrix);
        }
    }
    
////////////////////////////////////////////////////////////////////
    SCamera::SCamera(): m_priv(new(S3DView)) {
    
    }
    
    SCamera::~SCamera() {
        delete m_priv;
    }
    
    void SCamera::save() {
        m_priv->save();
    }
    
    void SCamera::restore() {
        m_priv->restore();
    }
    
    void SCamera::translate(float x, float y, float z) {
        m_priv->translate(x, y, z);
    }
    
    void SCamera::rotateX(float deg) {
        m_priv->rotateX(deg);
    }
    
    void SCamera::rotateY(float deg) {
        m_priv->rotateY(deg);
    }
    
    void SCamera::rotateZ(float deg) {
        m_priv->rotateZ(deg);
    }
    
    void SCamera::setCameraLocation(float x, float y, float z) {
        m_priv->setCameraLocation(x, y, z);
    }
    
    float SCamera::getCameraLocationX() {
        return m_priv->getCameraLocationX();;
    }
    
    float SCamera::getCameraLocationY() {
        return m_priv->getCameraLocationY();
    }
    
    float SCamera::getCameraLocationZ() {
        return m_priv->getCameraLocationZ();
    }
    
    void SCamera::getMatrix(SMatrix *out) const {
        return m_priv->getMatrix(out);
    }
    
}//end of namespace SOUI
